#include "GrahamScanConvexHull.h"
#include <TGS/GeoMath2.0/Point.h>
#include <vector>
#include <iterator>
#include <algorithm>
#include <deque>
#include <cmath>

using namespace std;

namespace
{
	struct gs_point2d
	{
	public:
		gs_point2d(double _x = 0.0, double _y = 0.0, double _angle = 0.0) : x(_x), y(_y), angle(_angle){}
		double x;
		double y;
		double angle;
	};

	const double _180DivPI  = 57.295779513082320876798154814105000;
	const int    counter_clock_wise = +1;
	const int    clock_wise         = -1;

	class GSPoint2DCompare
	{
	public:

		GSPoint2DCompare(gs_point2d* _anchor):anchor(_anchor){};

		bool operator()(const gs_point2d& p1, const gs_point2d& p2)
		{
			if (p1.angle < p2.angle)      return true;
			else if (p1.angle > p2.angle) return false;
			else if (is_equal(p1,p2))     return false;
			else if (lay_distance(anchor->x, anchor->y, p1.x, p1.y) < lay_distance(anchor->x, anchor->y, p2.x, p2.y))
				return true;
			else
				return false;
		}

	private:

		inline bool is_equal(const gs_point2d p1, gs_point2d p2)
		{
			return  is_equal(p1.x,p2.x) && is_equal(p1.y,p2.y);
		}

		inline bool is_equal(const double v1, const double& v2, const double epsilon = 1.0e-12)
		{
			double diff = v1 - v2;

			return  (-epsilon <= diff) && (diff <= epsilon);
		}

		inline double lay_distance(const double& x1, const double& y1, const double& x2, const double& y2)
		{
			double dx = (x1 - x2);
			double dy = (y1 - y2);

			return (dx * dx + dy * dy);
		}

		gs_point2d* anchor;

	};

	typedef TGS::GeoMath::GrahamScanConvexHull::PointContainer PointContainer;
	void graham_scan(std::vector<gs_point2d>& point, PointContainer& final_hull);

	inline double cartesian_angle(double x, double y);

	inline int orientation(const gs_point2d& p1,
		const gs_point2d& p2,
		const gs_point2d& p3);

	inline int orientation(const double x1, const double y1,
		const double x2, const double y2,
		const double px, const double py);

	inline bool is_equal(const double v1, const double& v2, const double epsilon = 1.0e-12);
}

namespace TGS{
namespace GeoMath{


GrahamScanConvexHull::GrahamScanConvexHull(void)
{
}


GrahamScanConvexHull::~GrahamScanConvexHull(void)
{
}

bool GrahamScanConvexHull::operator()(const PointContainer& points, PointContainer& finalHull)
{
	finalHull.clear();

	if (points.size() <= 3)
	{
		std::copy(points.begin(), points.end(), std::back_inserter(finalHull));

		return true;
	}

	unsigned int j = 0;
	gs_point2d   tmp_pnt;

	std::vector<gs_point2d> point;
	for(unsigned int i = 0; i < points.size(); i++)
	{
		point.push_back(gs_point2d(points[i].getX(), points[i].getY(), 0.0));

		if (point[i].y < point[j].y)
			j = i;
		else if (point[i].y == point[j].y)
			if (point[i].x < point[j].x)
				j = i;
	}

	tmp_pnt  = point[0];
	point[0] = point[j];
	point[j] = tmp_pnt;

	gs_point2d anchor = point[0];

	for (unsigned int i = 1; i < point.size(); i++)
	{
		point[i].angle = cartesian_angle(point[i].x - anchor.x, point[i].y - anchor.y);
	}

	sort(++point.begin(),point.end(),GSPoint2DCompare(&anchor));

	graham_scan(point, finalHull);

	return true;
}

}
}

namespace
{

	typedef TGS::GeoMath::GrahamScanConvexHull::PointContainer PointContainer;

	double cartesian_angle(double x, double y)
	{
		if      ((x >  0.0) && (y >  0.0)) return (atan( y / x) * _180DivPI);
		else if ((x <  0.0) && (y >  0.0)) return (atan(-x / y) * _180DivPI) +  90.0;
		else if ((x <  0.0) && (y <  0.0)) return (atan( y / x) * _180DivPI) + 180.0;
		else if ((x >  0.0) && (y <  0.0)) return (atan(-x / y) * _180DivPI) + 270.0;
		else if ((x == 0.0) && (y >  0.0)) return  90.0;
		else if ((x <  0.0) && (y == 0.0)) return 180.0;
		else if ((x == 0.0) && (y <  0.0)) return 270.0;
		else
			return 0.0;
	}

	int orientation(const gs_point2d& p1, const gs_point2d& p2, const gs_point2d& p3)
	{
		return orientation(p1.x,p1.y,p2.x,p2.y,p3.x,p3.y);
	}

	int orientation(const double x1, const double y1,
					const double x2, const double y2,
					const double px, const double py)
	{
		double orin = (x2 - x1) * (py - y1) - (px - x1) * (y2 - y1);

		if (is_equal(orin,0.0))
			return 0;              /* Orientaion is neutral aka collinear  */
		else if (orin < 0.0)
			return -1;             /* Orientaion is to the right-hand side */
		else
			return +1;             /* Orientaion is to the left-hand side  */

	}


	bool is_equal(const double v1, const double& v2, const double epsilon)
	{
		double diff = v1 - v2;
		return  (-epsilon <= diff) && (diff <= epsilon);
	}

	void graham_scan(std::vector<gs_point2d>& point, PointContainer& final_hull)
	{
		const std::size_t HEAD     = 0;
		const std::size_t PRE_HEAD = 1;

		std::deque<gs_point2d> pnt_queue;

		pnt_queue.push_front(point[0]);
		pnt_queue.push_front(point[1]);

		unsigned int i = 2;

		while(i < point.size())
		{
			if (pnt_queue.size() > 1)
			{
				if (orientation(pnt_queue[PRE_HEAD],pnt_queue[HEAD],point[i]) == counter_clock_wise)
					pnt_queue.push_front(point[i++]);
				else
					pnt_queue.pop_front();
			}
			else
				pnt_queue.push_front(point[i++]);
		}

		for(std::deque<gs_point2d>::iterator it = pnt_queue.begin(); it != pnt_queue.end(); it++)
		{
			final_hull.push_back(TGS::GeoMath::Point((*it).x, (*it).y));
		}
	}
}